﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PriorityQueue
{
    /***
     *    0
     *  1   2
     * 3 4 5 6
     * 
     * 
     **/

    public class PriorityQueue<T>
        where T : IComparable
    {        
        public PriorityQueue(int capacity = 0)
        {
            list = new List<T>(capacity);
        }

        public void Enqueue(T t)
        {
            list.Add(default(T));
            int pos = list.Count - 1;
            while (pos > 0)
            {
                int parent = (pos-1) / 2;
                if (t.CompareTo(list[parent]) < 0)
                {
                    list[pos] = list[parent];
                    pos = parent;
                }
                else
                {
                    break;
                }
            }
            list[pos] = t;
        }

        public void Dequeue()
        {
            if (list.Count==0)
            {
                throw new IndexOutOfRangeException("The queue is empty");
            }

            list[0] = list.Last();
            list.RemoveAt(list.Count-1);

            int pos = 0;
            while (2 * pos + 1 < list.Count)
            {
                int min = pos;
                
                if (2 * pos + 1 < list.Count && list[2 * pos + 1].CompareTo(list[min]) < 0)
                {
                    min = 2 * pos + 1;
                }

                if (2 * pos + 2 < list.Count && list[2 * pos + 2].CompareTo(list[min]) < 0)
                {
                    min = 2 * pos + 2;
                }

                if (min != pos)
                {
                    T t = list[min];
                    list[min] = list[pos];
                    list[pos] = t;
                    pos = min;
                }
                else
                {
                    break;
                }
            }
        }

        public T Top()
        {
            if (list.Count > 0)
            {
                return list[0];
            }
            else
            {
                throw new IndexOutOfRangeException("The queue is empty");
            }
        }

        private static int Compare(T t1, T t2)
        {
            if (t1 == null && t2 == null)
            {
                throw new InvalidOperationException("Invalid comparison between two null objects");
            }
            else if (t1 == null)
            {
                return 1;
            }
            else if (t2 == null)
            {
                return -1;
            }

            return t1.CompareTo(t2);
        }

        private List<T> list;

    }

    class Program
    {
        static void Main(string[] args)
        {
            PriorityQueue<int> pq = new PriorityQueue<int>(8);
            int[] arr = new[] { 3, 4, 5, 2, 1, 6, 8, 7, 9, 10, 45, 20, 4, 6, 7, 2 };
            foreach (var a in arr)
            {
                pq.Enqueue(a);
            }

            try
            {
                while (true)
                {
                    int a = pq.Top();
                    Console.WriteLine("{0}", a);
                    pq.Dequeue();
                }
            }
            catch
            {
            }
        }
    }
}
